Прямая задача кинематики SCARA

In [5]:
from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook
In [6]:
from kinematics import Vector, Quaternion, Transform
import graphics

Решение прямой задачи кинематики для манипулятора SCARA

Манипулятор кинематической схемы SCARA обладает четыремя степенями подвижности.

внеший вид SCARA

Можно выделить высоту колонны, длины первого и второго звеньев.

Обобщенные координаты будут в радианах и метрах:

Обобщенная координата Обозначение Размерность
Вращение вокруг колонны $q_0$ радиан
Вращение в локте $q_1$ радиан
Вращение инструмента $q_2$ радиан
Перемещение инструмента $q_3$ метр
In [7]:
def scara_chain(q, l, math_source=np):
    base = Transform.identity()
    column = base + Transform(
        Vector(0, 0, l[0]),
        Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source)
    )
    elbow = column + Transform(
        Vector(l[1], 0, 0),
        Quaternion.from_angle_axis(q[1], Vector(0, 0, 1), math_source)
    )
    tool = elbow + Transform(
        Vector(l[2], 0, 0),
        Quaternion.from_angle_axis(q[2], Vector(0, 0, 1), math_source)
    )
    flange = tool + Transform(
        Vector(0, 0, -q[3]),
        Quaternion.identity()
    )
    return [
        base,
        column,
        elbow,
        tool,
        flange
    ]

Зададим закон изменения обобщенных координат:

In [8]:
def scara_q(t, total):
    omega = t / total * np.pi * 2
    return [
        np.pi / 4 * np.sin(omega),
        np.pi / 2,
        omega,
        3 + 3 * np.cos(omega)
    ]

Укажем длины звеньев:

In [9]:
scara_l = [8, 4, 3]
In [22]:
scara_fig = plt.figure()
ax = scara_fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)

total = 100

def animate(frame):
    chain = scara_chain(scara_q(frame, total), scara_l)
    (x, y, z) = graphics.chain_to_points(chain)
    lines.set_data_3d(x, y, z)
    global r, g, b
    r.remove(); g.remove(); b.remove()
    r, g, b = graphics.axis(ax, chain[-1], 0.5)

    
animate(0)
fps = 25
scara_ani = animation.FuncAnimation(
    scara_fig,
    animate,
    frames=total,
    interval=1000.0/fps
)
In [25]:
HTML(scara_ani.to_jshtml())
Out[25]:

Отображение траектории

In [26]:
t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
x, y, z = graphics.chain_to_points(chain)

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
ax.plot(x[2], y[2], z[2], color="#c0c0c0")
ax.plot(x[3], y[3], z[3], color="#c0c0c0")
ax.plot(x[4], y[4], z[4], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
fig.show()

Определим диапазон изменения $Z$ координаты фланца:

In [14]:
print(
    "Z координата менялась в диапазоне от",
    np.min(x[-1]),
    "до",
    np.max(x[-1])
)
Z координата менялась в диапазоне от 0.7071067811865479 до 4.999999405196338

Измерение скорости движения

Численно продиффиринцируем для всех точек траектории:

In [15]:
t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
end = chain[-1]
velocity_x = np.diff(end.translation.x) / step
velocity_y = np.diff(end.translation.y) / step
velocity_z = np.diff(end.translation.z) / step
velocity_total = (velocity_x ** 2 + velocity_y ** 2 + velocity_z ** 2) ** 0.5

fig = plt.figure()
ax = fig.add_subplot()

ax.plot(t[:-1], velocity_x, color="#ff0000", label="$V_x$")
ax.plot(t[:-1], velocity_y, color="#00ff00", label="$V_y$")
ax.plot(t[:-1], velocity_z, color="#0000ff", label="$V_z$")
ax.plot(t[:-1], velocity_total, color="#000000", label="$V$")
fig.legend()
fig.show()

Оценим максимальную скорость на траектории:

In [16]:
index_max = np.argmax(velocity_total)
print(
    "Максимальная скорость была",
    velocity_total[index_max],
    "в момент",
    index_max * step,
    "сек"
)
Максимальная скорость была 2.4673894680006008 в момент 5.0 сек

Аналитическое решение прямой задачи кинематики

In [17]:
import sympy as sp
In [18]:
l_0, l_1, l_2 = sp.symbols("l_0, l_1, l_2")
q_0, q_1, q_2, q_3 = sp.symbols("q_0, q_1, q_2, q_3")
In [19]:
flange = scara_chain([q_0, q_1, q_2, q_3], [l_0, l_1, l_2], sp)[-1]
sp.simplify(flange.translation.x)
Out[19]:
$\displaystyle - l_{1} \sin^{2}{\left(\frac{q_{0}}{2} \right)} + l_{1} \cos^{2}{\left(\frac{q_{0}}{2} \right)} - l_{2} \sin^{2}{\left(\frac{q_{0}}{2} + \frac{q_{1}}{2} \right)} + l_{2} \cos^{2}{\left(\frac{q_{0}}{2} + \frac{q_{1}}{2} \right)}$
In [20]:
sp.simplify(flange.translation.y)
Out[20]:
$\displaystyle l_{1} \sin{\left(q_{0} \right)} + l_{2} \sin{\left(q_{0} + q_{1} \right)}$
In [21]:
sp.simplify(flange.translation.z)
Out[21]:
$\displaystyle l_{0} - q_{3}$

Самостоятельные задания

Для своего закона изменения обобщенных координат

  • Оцените диапазон $X$, $Y$ и $Z$ координат для данной траектории;
  • Оцените максимальную скорость по осям;
  • Оцените абсолютную максимальную скорость.
In [ ]: